#include "System.h"

Encoder OriginalEncoder;

/**************************************************************************
函数功能：读取编码器数值并计算车轮速度，单位m/s
入口参数：无
返回  值：无
**************************************************************************/
void Get_Velocity_Form_Encoder(void)
{   
  //获取编码器的原始数据
	//1ms 采集的脉冲值
	float Encoder_3_pr,Encoder_4_pr,Encoder_5_pr,Encoder_8_pr; 
	OriginalEncoder.A=Read_Encoder(3);	
	OriginalEncoder.B=Read_Encoder(4);	
	OriginalEncoder.C=Read_Encoder(5);	
	OriginalEncoder.D=Read_Encoder(8);	

	//根据不同小车型号决定编码器数值极性
	switch(Car_Mode)
	{
		case Omni3_Car:       Encoder_3_pr=-OriginalEncoder.A; Encoder_4_pr=-OriginalEncoder.B; 
							Encoder_5_pr= OriginalEncoder.C;  
		break; 
		case Omni4_Car:      Encoder_3_pr= OriginalEncoder.A; Encoder_4_pr= OriginalEncoder.B;
							Encoder_5_pr= OriginalEncoder.C;  Encoder_8_pr= OriginalEncoder.D;
		break;
	}
	//MOTOR_4.speed = (int)(d * Speedpa_TIM4 * 1000);	          
	//MOTOR_5.speed = (int)(d * Speedpa_TIM5 * 1000);					//【速度（m/s）】 = 【1个脉冲前进的距离（m）】*【单位时间采集的脉冲（ms）】*【1000】
	//MOTOR_8.speed = (int)(d * Speedpa_TIM8 * 1000);					//四个轮子的速度(m/s)		
	
	//ss += V4*0.001;
	
	//编码器原始数据转换为车轮速度，单位cm/s
	MOTOR_3.speed= Encoder_3_pr; 
	MOTOR_4.speed= Encoder_4_pr; 
	MOTOR_5.speed= Encoder_5_pr; 
	MOTOR_8.speed= Encoder_8_pr; 
}



/**************************************************************************
函数功能：读取编码器计数
入口参数：定时器
返回  值：编码器数值(代表速度)
**************************************************************************/
int Read_Encoder(uint8_t TIMX)
{
 int Encoder_TIM;    
 switch(TIMX)
 {
	case 3:  Encoder_TIM= (int)(short)TIM3 -> CNT;   TIM3 -> CNT=CNT_INIT;  break;
	case 4:  Encoder_TIM= (int)(short)TIM4 -> CNT;   TIM4 -> CNT=CNT_INIT;  break;
	case 5:  Encoder_TIM= (int)(short)TIM5 -> CNT;   TIM5 -> CNT=CNT_INIT;  break;	
	case 8:  Encoder_TIM= (int)(short)TIM8 -> CNT;   TIM8 -> CNT=CNT_INIT;  break;	
	default: Encoder_TIM=0;
 }
	return Encoder_TIM;
}